#ifndef RSIM_RIGIDBODY_H
#define RSIM_RIGIDBODY_H

/* -------------------------------------------------------------------------- *
 * File: RigidBody.h                                                          *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Transform.h"
#include "Vect3.h"
#include "Vect6.h"
#include "Joint.h"
#include "JointType.h"
#include "RigidBodyNode.h"

namespace RSIM{

class Joint;
class PrescribedMotion;
class QIndex;
class UIndex;
class BodyIndex;
class ExternalSpatialForce;
class SystemAcceleration;
class EOMData;
class ABAData;
class MassProperties;
        
/** Rigid body */
class RigidBody{
        private:
                /** Mass of this body */
                double Mass_;
                
                /** Inertia of this body */
                Mat3x3 Inertia_;
                
                /** Each body is attached to the parent via joint J */
                Joint *J_;
                
                /**
                This is the transform between frames B and F. The vector 
                T_BF::P_ is specified in frame B. The Rotation matrix 
                T_BF::R_ gives R_FB. 
                Shift tranform for body B (between frames B and F). It is
                assumed that the center of mass is located at B. 
                */
                Transform T_BF_;		
                
                /**
                This is the transform between frames P and M for the PARENT of this body. 
                The vector T_PM::P_ is specified in frame P. The Rotation matrix T_PM::R_ gives 
                R_PM, Shift tranform for Parent of B (between frames P and M)
                */
                Transform T_PM_;
                
                /** This transform is between F and M frames of the <b>SAME</b> body. If the body has no
                children, this is just a NULL pointer.*/
                Transform *T_FM_;
                
                /** Rotation matrix between the newtonian frame and body fixed frame F. That is if a vector is expressed in
                the newtonian frame as v_N then (v_N = R_NF_ * v_F) */
                Rotation R_NF_;
                
                /** This is the rotation matrix between F of child and parent. If a vector v is expressed in F of child as v_F then
                {v_F}_P = prF_C_F * v_F */
                Rotation prF_C_F_;
                
                /** User defined external spatial force */
                ExternalSpatialForce *spF_;
                
                /** Structure to store equations of motion for this body*/
                EOMData *EOM_;
                
                /** Structure to store various qunatities required in Articulated Body Algorithm*/
                ABAData *ABA_;

                /** Absolute angular velocity of body expressed in frame F of the body */
                Vect3 w_;
                
                /** Absolute linear velocity of the origin of the body expressed in F frame */
                Vect3 v_;

                /** Position vector which locates center of mass of this body from the F frame of this body
                expressed in F frame of this body */
                // This is used in shifting the body forces to the F frame of this body which must be done at each time step
                Vect3 rk_;

                /** A^{k}_1 = (S^k_{\beta})^T A^{k-1}_1 + P^{k}_{k} dot(u^{k}) + sAkt_ */
                Vect6 sAkt_;		

                /** Number of child bodies*/
                int nChildren_;
                
                JointType JT_;
                
                // To be removed in future versions.
                double betak_[3];
                
                Vect3 nbetak_;
                
                /** This rigid body is the pChildID_ ^th child of the parent. 
                For the root body, this value is set to -1*/
                // this also to be modified in future version.
                int pChildID_;
        protected:
                
                /** P = [Wx][J][W] */
                void Wx_J_W(const double *w, const double *J, double *p);
                
        public:
                /** Explicity specify both the transforms. This should be the preferred constructor for
                most of the applications. */
                RigidBody(const char *JointType, const MassProperties& MP, const Transform& Body_T_BF, const Transform& Parent_T_PM);
                
                /** Deep copy */
                RigidBody(const RigidBody& RB);
                
                ~RigidBody();
                
                const int& getpChildID()const{return pChildID_;}
                
                int& wgetpChildID(){return pChildID_;}
                
                /** Set the location and orientation of the frame attached to the center of mass
                with respect to body origin. If not set, this is a zero transform. */
                void setCOMPosition(const Transform& T);
                
                /** Update the joint Transform T_MF_ */
                void updJointTransform(const double *q, const RigidBodyNode* Parent);
                
                void initBetaK(const double *q, const RigidBodyNode* Parent);
                
                /** Partial of spatial relative joint velocity M_V_F with respect to u of the joint specified 
                by the joint index. hptr must be atleast of length 6. (e.g. double hptr[6]) */
                void getHColumn(const int& uindex, 
                                double *hptr)const;
                
                void setBetaK(const double *beta);
                
                void setQFromPrescribedMotion(State& state);
                
                void setUFromPrescribedMotion(State& state);
                
                /** updates linear and angular velocity of the origin of this body from state.*/
                void updSpatialVelocity(const State& state, 
                                        const RigidBodyNode *Parent, 
                                        const int& ChildID);
                
                /** Result is the same as above but will work just for the root body */
                void updRelVelocityOfRoot(const State& state);
                
                /** Add prescribed motion on the joint connecting this body to the parent. */
                void addPrescribedMotion(PrescribedMotion* Ptr);
                
                void stopPrescribedMotion();
                
                void continuePrescribedMotion();
                
                void addExternalSpatialForce(ExternalSpatialForce *F);
                
                /** get writable pointer to transform FM. */
                Transform* wgetBodyTransformFM();
                
                /** 
                Allocate space for parts of equations of motion which are
                constant in body basis. These are computed as a preprocessing 
                step and stored.
                */
                void initEOM();
                
                void initJoint(const int& NodeIndex);
                
                const JointType getJointType()const;
                
                // Sets constraint force cache to zero
                void formEOM(const SystemAcceleration *SysAccl);
                
                void sweepBackwardRNEA(const int& ChildID, RigidBodyNode *Parent, State& state);
                
                void sweepForwardRNEA(const RigidBodyNode *Parent, 				      
                                const int& ChildID,
                                const SystemAcceleration *SysAccl,
                                RigidBodyNode *Body,
                                State& state);
                
                /** 
                Allocate space for quantities which are required for doing forward/hybrid
                dynamics using Articulated Body Algorithm
                */
                void initForwardDynamics(const int& nchildren);		
                
                const bool areQuaternionsInUse()const;
                
                void setNQ(int *q)const;
                
                void setNU(int *u)const;
                
                void setDefaultQ(double *q)const;
                
                void setDefaultU(double *u)const;								
                
                void updOneQ(State& state, const QIndex& qindex, const double& q);
                
                void updOneU(State& state, const UIndex& uindex, const double& u);
                        
                /** @return Writable pointer to Q's associated with this body */
                double* wgetQ(State& state);
                
                /** @return Writable pointer to QDot's associated with this body */
                double* wgetQDot(State& state);
                
                /** @return Writable pointer to U's associated with this body */
                double* wgetU(State& state);
                
                /** @return Writable pointer to UDot's associated with this body */
                double* wgetUDot(State& state);
                
                const int getNChildren()const;
                
                void setNChildren(const int nchildren);
                                
                void sweepBackwardsABA(const RigidBodyNode *Body, 
                                const int& ChildID,
                                RigidBodyNode *Parent, 
                                State& state);
                
                void sweepForwardABA(const RigidBodyNode *Parent, const int& ChildID, State& state);
                
                Rotation& wgetRotationNF();
                
                void setRotationNF(const Mat3x3&);
                
                /** To be used only if this body is the root and connected directly to ground*/
                void setRotationNF();
                
                void allocateSpaceForTransformFM(const int nchildren);
                
                Vect3 expressAngularVelocityInGround() const;
                
                void setBodyRK(const double *rk);
                
                const Vect3& getnbetak()const;
                Vect3& wgetnbetak();
                
                const double* getBetaK()const;

                const double& getMass() const ;
                
                void calcShiftPartialLinear(const Vect3& beta,
                                            const int& uindex,
                                            double *dv)const;
                
                ABAData* wgetABAData(); 
                
                const ABAData* getABAData()const;
                
                const EOMData* getEOMData()const;
                
                EOMData* wgetEOMData();
                
                const Mat3x3& getInertia() const;
                
                const Joint* getJoint() const ;
                
                const Transform& getTransformBF() const;
                
                const Transform& getTransformPM() const;
                
                const Transform& getTransformBCOM() const;
                
                const Vect3& getAngularVelocity() const;
                
                const Vect3& getLinearVelocity() const;
                
                const Vect3& getBodyRK() const;
                
                /** Same as getNDOF() */
                int getDOF()const;
                
                /** Same as getDOF() */
                int getNDOF()const;
                
                const int getNQ(const State& state)const;
                
                const int getNU(const State& state)const;
                
                const double* getQ(const State& state) const;
                
                const double* getU(const State& state) const;
                
                const double* getUDot(const State& state) const;
                
                const double* getQDot(const State& state) const;
                
                void getConstraintForce(double *Fc) const;
                
                const double getOneU(const State& state,const UIndex& uindex) const;
                
                const double getOneQ(const State& state,const QIndex& qindex) const;
                
                const Rotation& getJointRotationMF() const;
                
                const Transform& getJointTransformMF() const;
                
                const Transform* getBodyTransformFM() const;
                
                const Rotation& getRotationNF() const;
                
                const Rotation& getRotationFCF() const;
                
                Rotation& wgetRotationFCF();
                
                const Vect6& getAkt()const;
                
                // Debug functions
                void printInfo();
                void printJointType();
};

inline const Vect6& RigidBody::getAkt()const{return sAkt_;}

inline void RigidBody::calcShiftPartialLinear(const Vect3& beta,const int& uindex, double *dv)const{        
        J_->calcShiftPartialLinear(beta,uindex,R_NF_,dv);
}

inline void RigidBody::getHColumn(const int& uindex,double *hptr)const{
        J_->getHColumn(uindex,hptr);
}

inline Vect3& RigidBody::wgetnbetak(){return this->nbetak_;}

inline const Vect3& RigidBody::getnbetak()const{return this->nbetak_;}

inline ABAData* RigidBody::wgetABAData(){return this->ABA_;}

inline const ABAData* RigidBody::getABAData()const{return this->ABA_;}

inline const EOMData* RigidBody::getEOMData()const{return this->EOM_;}

inline EOMData* RigidBody::wgetEOMData(){return this->EOM_;}

inline Transform* RigidBody::wgetBodyTransformFM(){return T_FM_;}

inline const double& RigidBody::getMass() const {return Mass_;}

inline const Mat3x3& RigidBody::getInertia() const {return Inertia_;}

inline const Joint* RigidBody::getJoint() const {return J_;}

inline const Rotation& RigidBody::getRotationFCF() const{return this->prF_C_F_;}

inline Rotation& RigidBody::wgetRotationFCF(){return this->prF_C_F_;}

inline const Rotation& RigidBody::getRotationNF() const{return R_NF_;}

inline const Transform& RigidBody::getTransformBF() const {return T_BF_;}

inline const Transform& RigidBody::getTransformPM() const {return T_PM_;}

inline Rotation& RigidBody::wgetRotationNF(){return R_NF_;}

inline const Vect3& RigidBody::getAngularVelocity()const{return this->w_;}

inline const Vect3& RigidBody::getLinearVelocity()const{return this->v_;}

/** Updates the angular and linear velocity  */
inline void RigidBody::updSpatialVelocity(const State& state, const RigidBodyNode *Parent, const int& ChildID){
        this->J_->updVelocity(state, Parent, prF_C_F_, betak_, w_, v_);
}

inline void RigidBody::updRelVelocityOfRoot(const State& state){this->J_->updRelVelocity(state, this->w_,this->v_);}

inline const Transform* RigidBody::getBodyTransformFM() const {return this->T_FM_;}

inline const Vect3& RigidBody::getBodyRK() const {return this->rk_;}

inline void RigidBody::setQFromPrescribedMotion(State& state){
        J_->setQFromPrescribedMotion(state);
}

inline void RigidBody::setUFromPrescribedMotion(State& state){
        J_->setUFromPrescribedMotion(state);
}

inline void RigidBody::updJointTransform(const double *q, const RigidBodyNode *Parent){
        J_->updJointTransform(q,Parent,pChildID_, betak_);
}

inline void RigidBody::initBetaK(const double *q, const RigidBodyNode *Parent){
        J_->updBetaK(q,Parent,pChildID_, betak_);
}

inline void RigidBody::setBetaK(const double *beta){
        betak_[0] = beta[0];
        betak_[1] = beta[1];
        betak_[2] = beta[2];
}

inline void RigidBody::addPrescribedMotion(PrescribedMotion* Ptr){
        J_->addPrescribedMotion(Ptr);
}

inline void RigidBody::stopPrescribedMotion(){
        J_->stopPrescribedMotion();
}
                
inline void RigidBody::continuePrescribedMotion(){
        J_->continuePrescribedMotion();
}

inline void RigidBody::addExternalSpatialForce(ExternalSpatialForce *F){
        spF_ = F;
}

inline void RigidBody::updOneQ(State& state, const QIndex& qindex, const double& q){
        J_->updOneQ(state, qindex,q);
}

inline void RigidBody::updOneU(State& state, const UIndex& uindex, const double& u){
        J_->updOneU(state, uindex,u);
}

inline const int RigidBody::getNChildren()const{
        return this->nChildren_;
}

inline void RigidBody::setNChildren(const int nchildren){
        nChildren_ = nchildren;
}

inline const JointType RigidBody::getJointType()const{return JT_;}

inline void RigidBody::allocateSpaceForTransformFM(const int nchildren){T_FM_ = new Transform [nchildren];}

inline double* RigidBody::wgetQ(State& state){return(J_->wgetQ(state));}

inline double* RigidBody::wgetQDot(State& state){return(J_->wgetQDot(state));}

inline double* RigidBody::wgetU(State& state){return(J_->wgetU(state));}

inline double* RigidBody::wgetUDot(State& state){return(J_->wgetUDot(state));}

inline const double* RigidBody::getQ(const State& state) const {return J_->getQ(state);}

inline const double* RigidBody::getU(const State& state) const {return J_->getU(state);}

inline const double* RigidBody::getUDot(const State& state) const {return J_->getUDot(state);}

inline const double* RigidBody::getQDot(const State& state) const{return J_->getQDot(state);}

inline const double RigidBody::getOneU(const State& state,const UIndex& uindex) const {return J_->getOneU(state,uindex);}

inline const double RigidBody::getOneQ(const State& state,const QIndex& qindex) const {return J_->getOneQ(state, qindex);}

inline const Rotation& RigidBody::getJointRotationMF() const {return J_->getRotationMF();}

inline const Transform& RigidBody::getJointTransformMF() const{return this->J_->getTransformMF();}

inline const int RigidBody::getNQ(const State& state)const{return J_->getNQ(state);}

inline const double* RigidBody::getBetaK()const{return this->betak_;}

inline const int RigidBody::getNU(const State& state)const{return J_->getNU(state);}

inline int RigidBody::getDOF()const{return J_->getDOF();}

inline int RigidBody::getNDOF()const{return J_->getDOF();}

inline void RigidBody::setRotationNF(const Mat3x3& NF){R_NF_ = NF;}

inline void RigidBody::setNQ(int *q)const{J_->setNQ(q);}
                
inline void RigidBody::setNU(int *u)const{J_->setNU(u);}

inline void RigidBody::setDefaultQ(double *q)const{J_->setDefaultQ(q);}
                
inline void RigidBody::setDefaultU(double *u)const{J_->setDefaultU(u);}

inline const bool RigidBody::areQuaternionsInUse()const{return J_->areQuaternionsInUse();}

inline void RigidBody::sweepBackwardsABA(	const RigidBodyNode *Body,
                                                const int& ChildID,
                                                RigidBodyNode *Parent,
                                                State& state){		
                
        this->J_->calcAKT(Parent, Body, ChildID, this->betak_, state, this->sAkt_);
        
        this->J_->sweepBackwards(nChildren_, ChildID, this->sAkt_,this->prF_C_F_,this->EOM_,this->betak_,Parent,this->ABA_);
}

} // namespace RSIM
#endif